#include <set>
#include "distancetransform.h"
#include "../math/Vector3.h"

using namespace PBVP;

const float DistanceTransform::MAX_FLOAT = (float)1.0e12;
const double DistanceTransform::TOLERANCE = 1e-6;

DistanceTransform::DistanceTransform(GeometryData *pGeoData)
{
	m_pGeoData = pGeoData;
	// orient triangles
	// m_pGeoData->orientTriangles();

	// Set default extents of the distance transform
	Vector3 minExt, maxExt;
	pGeoData->getExtent(minExt, maxExt);
	Vector3 center = (minExt + maxExt) / 2;
	Vector3 half = (maxExt - minExt) / 2;
	// const float marchingDist = 40.f;
	half = half + 1.1*(half + 3);
	minExt = center - half;
	maxExt = center + half;
	for(int i = 0; i < 3; i++) {
		m_MinExt[i] = minExt[i];
		m_MaxExt[i] = maxExt[i];
	}

	m_Dim[0] = m_Dim[1] = m_Dim[2] = 96;
	p_Cells = NULL;
	m_pReg3Data = NULL;
	computeTriangleNormals();
}

DistanceTransform::~DistanceTransform()
{
	//if (m_pReg3Data)
	//{
	//	delete m_pReg3Data;
	//}
	if(p_Cells) delete[] p_Cells;
	if(m_pTriNormals) delete[] m_pTriNormals;
}

void DistanceTransform::computeTriangleNormals()
{
	m_pTriNormals = new Vector3[m_pGeoData->numOfTriangles()];

	//compute face normals
	for (unsigned int i = 0; i < m_pGeoData->numOfTriangles(); i++) {
		Vector3 v0, v1, v2;
		m_pGeoData->getTriangleVerts(i, v0, v1, v2);
		Vector3 tan1 = v1 - v0;
		Vector3 tan2 = v2 - v0;

		m_pTriNormals[i] = tan1.crossProduct(tan2);
		m_pTriNormals[i].normalize();
	}
}

bool DistanceTransform::pointInTriangle(const Vector3& res, const Vector3& vert0, 
										const Vector3& vert1, const Vector3& vert2, 
										const Vector3& norm)
{
	double alpha, beta;
	double u0, u1, u2, v0, v1, v2;
	int index;

	if(fabs(norm[0]) > fabs(norm[1])) {
		if(fabs(norm[0]) > fabs(norm[2])) {
			index = 0;
		} else {
			index = 2;
		}
	} else {
		if(fabs(norm[1]) > fabs(norm[2])) {
			index = 1;
		} else {
			index = 2;
		}
	}

	if( index == 0 ) {		// project to y-z plane
		u0 = res[1] - vert0[1]; 
		u1 = vert1[1] - vert0[1]; 
		u2 = vert2[1] - vert0[1]; 

		v0 = res[2] - vert0[2]; 
		v1 = vert1[2] - vert0[2]; 
		v2 = vert2[2] - vert0[2]; 
	}
	else if ( index == 1 ) {	// project to x-z plane
		u0 = res[2] - vert0[2]; 
		u1 = vert1[2] - vert0[2]; 
		u2 = vert2[2] - vert0[2];

		v0 = res[0] - vert0[0];
		v1 = vert1[0] - vert0[0];
		v2 = vert2[0] - vert0[0];
	} else 	{	// project to x-y plane
		u0 = res[0] - vert0[0];
		u1 = vert1[0] - vert0[0];
		u2 = vert2[0] - vert0[0];

		v0 = res[1] - vert0[1]; 
		v1 = vert1[1] - vert0[1]; 
		v2 = vert2[1] - vert0[1]; 
	}

	alpha = ( u0*v2 - v0*u2 ) / ( u1*v2 - v1*u2 );
	if( alpha < 0 ) return false;

	beta = ( u1*v0 - v1*u0 ) / ( u1*v2 - v1*u2 );
	if( beta < 0 ) return false;

	if( alpha+beta > 1) return false;

	return true;
}

double DistanceTransform::distance2Edge(const Vector3& pnt, const Vector3& v1, 
										const Vector3& v2, Vector3& ne)
{
	Vector3 edge(v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]);
	float len = edge.length();
	edge.normalize();

	Vector3 vec(pnt[0]-v1[0], pnt[1]-v1[1], pnt[2]-v1[2]);
	float dot = vec.dotProduct(edge);

	if(dot < 0) {
		ne = v1;
		return pnt.distance(v1);
	} else if(dot > len) {
		ne = v2;
		return pnt.distance(v2);
	}
	ne[0] = v1[0] + dot * edge[0]; 
	ne[1] = v1[1] + dot * edge[1];
	ne[2] = v1[2] + dot * edge[2];
	return pnt.distance(ne);
}

double DistanceTransform::distance2Triangle(const Vector3& pnt, int nt, Vector3& nearPnt)
{
	double  dist, temp[3];
	Vector3 v0, v1, v2;
	m_pGeoData->getTriangleVerts(nt, v0, v1, v2);
	Vector3 norm = getTriangleNormal(nt);
	Vector3 vnorm(norm), vdiff;

	Vector3 edgePnt;

	//1) First compute the shortest distance from the pnt to the plane of the Triangle.
	vdiff = pnt - v0;
	dist = vdiff.dotProduct(vnorm);
	nearPnt[0] = pnt[0] - dist * norm[0];
	nearPnt[1] = pnt[1] - dist * norm[1];
	nearPnt[2] = pnt[2] - dist * norm[2];

	//2) Then check if the projected point is within the triangle or not. 
	//	  if yes, then the above is the correct shortest distance.
	if(pointInTriangle(nearPnt, v0, v1, v2, norm)) {
		return fabs(dist);
	}

	//3) now, the closest point must on the edge.
	// find the nearest point on each edge.
	dist = distance2Edge(pnt, v0, v1, nearPnt);
	double edgeDist = distance2Edge(pnt, v1, v2, edgePnt);
	if(edgeDist < dist) {
		dist = edgeDist;
		nearPnt = edgePnt;
	}
	edgeDist = distance2Edge(pnt, v2, v0, edgePnt);
	if(edgeDist < dist) {
		dist = edgeDist;
		nearPnt = edgePnt;
	}

	assert(dist >= 0);
	return dist;
}

Reg3DVolumeData* DistanceTransform::computeSDF()
{
	m_pReg3Data = new Reg3DVolumeData(m_Dim);
	m_pReg3Data->setNumOfVariables(1);
	int nverts = m_Dim[0]*m_Dim[1]*m_Dim[2];
	float *data = new float[nverts];
	memset(data, 0, sizeof(float)*nverts);
	m_pReg3Data->setDataArray(data);
	m_pReg3Data->setMinMaxExt(m_MinExt, m_MaxExt);

	init();

	int i, j, k;	
	int *parent = new int[nverts];
	for(i = 0; i < nverts; i++) {
		parent[i] = i;
	}

	float orig[3], span[3];
	m_pReg3Data->getSpan(span);
	m_pReg3Data->getOrigin(orig);

	// 1D distance transform along x
	float *f = new float[m_Dim[0]];
	float *d = new float[m_Dim[0]];
	int *p = new int[m_Dim[0]];
	for (k = 0; k < m_Dim[2]; k++) {
		for(j = 0; j < m_Dim[1]; j++) {	
			int index = m_pReg3Data->index2vert(0, j, k);
			for(i = 0; i < m_Dim[0]; i++) {
				f[i] = m_pReg3Data->getValue(index+i);
				p[i] = parent[index+i];
			}
			transform1D(m_Dim[0], f, d, p, span[0]);
			for(i = 0; i < m_Dim[0]; i++) {
				m_pReg3Data->setValue(d[i], index+i);
				parent[index+i] = p[i];
			}
		}
	}
	delete[] f;
	delete[] d;
	delete[] p;

	f = new float[m_Dim[1]];
	d = new float[m_Dim[1]];
	p = new int[m_Dim[1]];
	// 1D distance transform along y
	for(k = 0; k < m_Dim[2]; k++) {
		for(i = 0; i < m_Dim[0]; i++) {
			int index = m_pReg3Data->index2vert(i, 0, k);
			for(j = 0; j < m_Dim[1]; j++) {
				f[j] = m_pReg3Data->getValue(index+m_Dim[0]*j);
				p[j] = parent[index+m_Dim[0]*j];
			}
			transform1D(m_Dim[1], f, d, p, span[1]);
			for(j = 0; j < m_Dim[1]; j++){
				m_pReg3Data->setValue(d[j], index+m_Dim[0]*j);
				parent[index+m_Dim[0]*j] = p[j];
			}
		}
	}
	delete[] f;
	delete[] d;
	delete[] p;

	f = new float[m_Dim[2]];
	d = new float[m_Dim[2]];
	p = new int[m_Dim[2]];

	for(j = 0; j < m_Dim[1]; j++) {
		for (i = 0; i < m_Dim[0]; i++) {
			int index = m_pReg3Data->index2vert(i, j, 0);
			for(k = 0; k < m_Dim[2]; k++) {
				f[k] = m_pReg3Data->getValue(index+k*m_Dim[0]*m_Dim[1]);
				p[k] = parent[index+k*m_Dim[0]*m_Dim[1]];
			}
			transform1D(m_Dim[2], f, d, p, span[2]);
			for(k = 0; k < m_Dim[2]; k++) {
				m_pReg3Data->setValue(d[k], index+k*m_Dim[0]*m_Dim[1]);
				parent[index+k*m_Dim[0]*m_Dim[1]] = p[k];
			}
		}
	}
	delete[] f;
	delete[] d;
	delete[] p;


	Vector3 *closest = new Vector3[nverts];
	// Compute the distance functions for points near the surface
	for (k = 0; k < m_Dim[2]; k++) {
		for(j = 0; j < m_Dim[1]; j++) {
			for (i = 0; i < m_Dim[0]; i++) {
				int nv = m_pReg3Data->index2vert(i, j, k);
				if(parent[nv] == nv) {	
					float d = computeNearDistance(i, j, k, closest[nv]);
					m_pReg3Data->setValue(d, nv);
				}
			}
		}
	}

	// update the distances for other points
	for (k = 0; k < m_Dim[2]; k++) {
		for(j = 0; j < m_Dim[1]; j++) {
			for (i = 0; i < m_Dim[0]; i++) {
				int nv = m_pReg3Data->index2vert(i, j, k);
				if (parent[nv] != nv) {
					Vector3 p(orig[0]+i*span[0], orig[1]+j*span[1], orig[2]+k*span[2]);
					float d = p.distance(closest[parent[nv]]);
					assert(parent[parent[nv]] == parent[nv]);
					if(m_pReg3Data->getValue(parent[nv]) < 0) {
						d = -d;
					}
					m_pReg3Data->setValue(d, nv);
				}
			}
		}
	}
	delete[] closest;
	delete[] parent;

	m_pReg3Data->calcMinMax();
	printf("min dist = %f , max dist = %f\n", m_pReg3Data->getMinValue(), m_pReg3Data->getMaxValue());
	
	return m_pReg3Data;
}

void DistanceTransform::transform1D(int n, float f[], float d[], int parent[], float span)
{
	int i, k = 0;		// Index of rightmost parabola in lower envelope
	int *v = new int[n];
	int *temp = new int[n];
	float *z = new float[n+1];
	float s2 = span*span;

	float min = f[0];
	d[0] = f[0];
	for (i = 1; i < n; i++) {
		if(f[i] < min) min = f[i];
		d[i] = f[i];
	}
	if(min >= MAX_FLOAT) return;		// every number is max, no need to transform.

	for(i = 0; i < n; i++) {
		if(f[i] < MAX_FLOAT) f[i] = f[i] / s2 ;
		temp[i] = parent[i];
	}

	v[0] = 0;
	z[0] = -MAX_FLOAT;
	z[1] = MAX_FLOAT;

	for(i = 1; i < n; i++) {
		//intersection of ith parabola and the current right most parabola 
		float s; 
		if(f[i] < MAX_FLOAT) {
			s = ((f[i] + i*i) - (f[v[k]] + v[k]*v[k])) / (2*i-2*v[k]);
		} else {
			s = MAX_FLOAT;
		}

		while (s <= z[k]) {
			k --;		// delete one parabola from the lower envelop
			s = ((f[i] + i*i) - (f[v[k]] + v[k]*v[k])) / (2*i-2*v[k]);
		}
		k ++;
		v[k] = i;
		z[k] = s;
		z[k+1] = MAX_FLOAT;
	}

	k = 0;
	for(i = 0; i < n; i++) {
		while (z[k+1] < i) {
			k++;
		}
		if(f[v[k]] < MAX_FLOAT) {
			d[i] = (i - v[k])*(i-v[k]) + f[v[k]];
			d[i] *= s2;
			parent[i] = temp[v[k]];
		} else {
			d[i] = MAX_FLOAT;
		}

	}

	delete[] temp;
	delete[] v;
	delete[] z;
}

float DistanceTransform::computeNearDistance(int ix, int iy, int iz, Vector3 &nearPnt)
{
	std::set<int> triSet;
	
	for(int kk = -1; kk <= 0; kk++) {
		for(int jj = -1; jj <= 0; jj++) {
			for(int ii = -1; ii <= 0; ii++) {
				int ci, cj, ck;
				ci = ix + ii;
				cj = iy + jj;
				ck = iz + kk;
				if(ci < 0 || ci >= m_Dim[0]-1 || cj < 0 || 
				   cj >= m_Dim[1]-1 || ck < 0 || ck >= m_Dim[2]-1) 
				{
					continue;
				}
				int nc = m_pReg3Data->index2cell(ci, cj, ck);
				for(int i = 0; i < p_Cells[nc].triList.size(); i++) {
					triSet.insert(p_Cells[nc].triList[i]);	
				}
			}
		}
	}
	std::set<int>::iterator it = triSet.begin();

	int nclose = 0;
	dynamic_array<int> nearTri;
	double dis = MAX_FLOAT;
	// double TOLERANCE = 1.0e-6;
	Vector3 vert;
	float orig[3], span[3];
	m_pReg3Data->getOrigin(orig);
	m_pReg3Data->getSpan(span);
	vert[0] = orig[0] + ix*span[0];
	vert[1] = orig[1] + iy*span[1];
	vert[2] = orig[2] + iz*span[2];
	while(it != triSet.end()) {
		Vector3 myNear;
		int tid = *it;
		assert(tid >= 0);
		double d = distance2Triangle(vert, tid, myNear);
		if(d <= dis - TOLERANCE) {	// new closest intersection
			dis = d;
			nearPnt = myNear;
			nearTri.clear();
			nclose = 0;
			nearTri[nclose++] = tid;
		} else if(fabs(d - dis) < TOLERANCE) {
			if( nearPnt.distance(myNear) < TOLERANCE) { // same intersection for another triangle
				nearTri[nclose++] = tid;
			}
		}
		++it;
	}
	// set the sign of the distance according to (p - p')*n to nearest plane
	//	printf("vert: (%f %f %f) has distance %f and nearest point (%f %f %f) on %d triangles\n", 
	//			vert[0], vert[1], vert[2], dis, nearPnt[0], nearPnt[1], nearPnt[2], nearTri.length());

	int id;
	if(nearTri.size() == 1) {
		id = 0;
		// Vector3 trinorm = m_pGeoData->getTriangleNormal(nearTri[id]);
		Vector3 trinorm = m_pTriNormals[nearTri[id]];
		Vector3 diff = vert - nearPnt;
		if (diff.dotProduct(trinorm) < 0) {
			dis = - dis;
		}
	} else {
		int sgn = inOrOut(nearTri, vert, nearPnt); 
		dis *= sgn;
	}

	return dis;
}

void DistanceTransform::init() {
	int i, j, k;
	float orig[3], span[3];
	m_pReg3Data->getOrigin(orig);
	m_pReg3Data->getSpan(span);

	p_Cells = new Cell[m_pReg3Data->numOfCells()];
	// build up the triangle list for cells
	for(int nt = 0; nt < m_pGeoData->numOfTriangles(); nt++) {
		Vector3 v0, v1, v2;
		m_pGeoData->getTriangleVerts(nt, v0, v1, v2);
		float lower[3], upper[3];
		for(j = 0; j < 3; j++) {
			lower[j] = v0[j];
			upper[j] = v0[j];
		}
		for(j = 0; j < 3; j++) {
			if(v1[j] < lower[j]) lower[j] = v1[j];
			else if(v1[j] > upper[j]) upper[j] = v1[j];
			if(v2[j] < lower[j]) lower[j] = v2[j];
			else if(v2[j] > upper[j]) upper[j] = v2[j];
		}

		int minId[3], maxId[3];
		for(j = 0; j < 3; j++) {
			minId[j] = (int)floor((lower[j]-orig[j]) / span[j]);
			maxId[j] = (int)floor((upper[j]-orig[j]) / span[j]);
		}
		for(k = minId[2]; k <= maxId[2]; k++) {
			for(j = minId[1]; j <= maxId[1]; j++) {
				for (i = minId[0]; i <= maxId[0]; i++) {
					int nc = m_pReg3Data->index2cell(i, j, k);
					assert(nc < m_pReg3Data->numOfCells() && nc >= 0);
					Vector3 norm = m_pTriNormals[nt];
					//p_Surf->getTriNormal(nt, norm);
					
					if(intersectCell(v0, norm, i, j, k)) {
						Vector3 lower, upper;
						lower[0] = orig[0] + i*span[0];
						lower[1] = orig[1] + j*span[1];
						lower[2] = orig[2] + k*span[2];
						upper[0] = lower[0] + span[0];
						upper[1] = lower[1] + span[1];
						upper[2] = lower[2] + span[2];
						if (TriangleCubeIntersection(nt, lower, upper)) {
							p_Cells[nc].triList.push_back(nt);
						}			
					}
				}
			}
		}	
	}

	// Initialize the distance function of points near the surface to 0
	for (k = 0; k < m_Dim[2]; k++) {
		for(j = 0; j < m_Dim[1]; j++) {
			for(i = 0; i < m_Dim[0]; i++) {
				int nv = m_pReg3Data->index2vert(i, j, k);
				if(isNearSurface(i, j, k)) {
					m_pReg3Data->setValue(0, nv);
				} else {
					m_pReg3Data->setValue(MAX_FLOAT, nv);
				}
			}
		}
	}
	// m_pReg3Data->init();
	printf("Initial distance min: %f, max = %f\n", 
			m_pReg3Data->getMinValue(), m_pReg3Data->getMaxValue());
}

double DistanceTransform::signedDist2Plane(const Vector3& pnt, const Vector3& v0, const Vector3& norm)
{
	Vector3 vdiff = pnt - v0;

	return vdiff.dotProduct(norm);
}

double DistanceTransform::rayTriangleIntersection(int nt, const Vector3& begin, const Vector3& end)
{
	//const double TOLERANCE = 1.0e-6;
	Vector3 v0, v1, v2;
	Vector3 norm = getTriangleNormal(nt);
	m_pGeoData->getTriangleVerts(nt, v0, v1, v2);

	double dbeg = signedDist2Plane(begin, v0, norm);
	double dend = signedDist2Plane(end, v0, norm);
	if(fabs(dbeg) < TOLERANCE) {
		if(pointInTriangle(begin, v0, v1, v2, norm)) return 0;
	}
	if(fabs(dend) < TOLERANCE) {
		if(pointInTriangle(end, v0, v1, v2, norm)) return 1;
	}
	if(dbeg * dend > 0) return MAX_FLOAT;	// no intersection
	if(fabs(dbeg) < TOLERANCE && fabs(dend) < TOLERANCE) return 0.5;  // a hack that needs to fixed later
	double denom = norm.dotProduct(end - begin);
	double t = - dbeg / denom;
	assert (t >= 0); 
	if(t > 1) t = 1;
	Vector3 intersect;
	intersect[0] = begin[0] + t*(end[0] - begin[0]);
	intersect[1] = begin[1] + t*(end[1] - begin[1]);
	intersect[2] = begin[2] + t*(end[2] - begin[2]);
	if(pointInTriangle(intersect, v0, v1, v2, norm)) return t;
	return MAX_FLOAT;
}

int DistanceTransform::inOrOut(const dynamic_array<int> &triList, const Vector3& vert, 
							   const Vector3& nearPnt)
{
	int i, j, len = triList.size();
	assert(len > 1);

	Vector3 center, cpnt;
	Vector3 v0, v1, v2;
	m_pGeoData->getTriangleVerts(triList[0], v0, v1, v2);
	double tnear = 1;
	int nearTri = 0;
	for(i = 0; i < 3; i++) {
		center[i] = (v0[i] + v1[i] + v2[i])/ 3;
		cpnt[i] = 0.9 * nearPnt[i] + 0.1 * center[i];
	}
	for(i = 1; i < len; i++) {
		double t = rayTriangleIntersection(triList[i], vert, cpnt);
		if(t < tnear) {
			tnear = t;
			nearTri = i;
		}
	}

	Vector3 trinorm = getTriangleNormal(triList[nearTri]);
	m_pGeoData->getTriangleVerts(triList[nearTri], v0, v1, v2);
	Vector3 diff = vert - v0;
	if (diff.dotProduct(trinorm) < 0) {
		return -1;
	}
	return 1;
}

bool DistanceTransform::isNearSurface(int i, int j, int k)
{
	for(int kk = -1; kk <= 0; kk++) {
		for(int jj = -1; jj <= 0; jj++) {
			for(int ii = -1; ii <= 0; ii++) {
				int ci, cj, ck;
				ci = i + ii;
				cj = j + jj;
				ck = k + kk;
				if(ci < 0 || ci >= m_Dim[0]-1 || cj < 0 || cj >= m_Dim[1]-1 || ck < 0 || ck >= m_Dim[2]-1) {
					continue;
				}
				int nc = m_pReg3Data->index2cell(ci, cj, ck);
				if(p_Cells[nc].triList.size() > 0) return true;
			}
		}
	}
	return false;
}

bool DistanceTransform::intersectCell(const Vector3& v0, const Vector3& norm, int i, int j, int k)
{
	Vector3 n(norm[0], norm[1], norm[2]);

	float xyz[3], orig[3], span[3];
	m_pReg3Data->getOrigin(orig);
	m_pReg3Data->getSpan(span);
	xyz[0] = orig[0] + i*span[0];
	xyz[1] = orig[1] + j*span[1];
	xyz[2] = orig[2] + k*span[2];
	Vector3 p(xyz[0]-v0[0], xyz[1]-v0[1], xyz[2]-v0[2]);
	float dot = n.dotProduct(p);

	for(int kk = 0; kk <= 1; kk++) {
		for(int jj = 0; jj <= 1; jj++) {
			for(int ii = 0; ii <= 1; ii++) {
				xyz[0] = orig[0] + (i+ii)*span[0];
				xyz[1] = orig[1] + (j+jj)*span[1];
				xyz[2] = orig[2] + (k+kk)*span[2];
				Vector3 p(xyz[0]-v0[0], xyz[1]-v0[1], xyz[2]-v0[2]);
				float dot2 = n.dotProduct(p);
				if(dot*dot2 <= 0) {		// two vertices on the opposite sides of the triangle plane
					return true;
				}
			}
		}
	}
	return false;
}

static bool isEqual (double one, double two)
{
	if ( (-DistanceTransform::TOLERANCE <= (one-two)) && 
		((one-two) <= DistanceTransform::TOLERANCE) )
	{
		return true;
	}
	return false;
}

static bool isZero(double num)
{
	if ( (-1*DistanceTransform::TOLERANCE <= num) && 
		(num <= DistanceTransform::TOLERANCE) ) 
	{
		return true;
	}
	return false;
}

static bool isNegative(double num)
{
	if (num <0)	return true;
	return false;
}

static bool isBetween(double one, double two, double num)
{
	if ( ((one<=num) && (num<=two)) || ((isEqual(num, one)) || (isEqual(num, two))) )
	{
		return true;
	}
	return false;
}

static bool isZero(const Vector3& vec)
{
	double val = sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);

	if (isZero(val))
		return true;
	return false;
}

static bool isSame(const Vector3& one, const Vector3& two)
{
	if (isZero(one - two)) return true;
	return false;
}

//static bool edgeIntersectSquare(const Point3f& p0, const Point3f& p1, 
static bool edgeIntersectSquare(double y0, double z0, double y1, double z1,
								double ymin, double ymax, double zmin, double zmax, int flag)
{
	//	double y0, y1, z0, z1;
	//	switch(flag) {
	//	case 1:			// Y-Z plane
	//		y0 = p0[1]; y1 = p1[1];
	//		z0 = p0[2]; z1 = p1[2];
	//		break;
	//	case 2:			// X-Z plane
	//		y0 = p0[0]; y1 = p1[0];
	//		z0 = p0[2]; z1 = p1[2];
	//		break;
	//	case 3:			// X-Y plane
	//		y0 = p0[0]; y1 = p1[0];
	//		z0 = p0[1]; z1 = p1[1];
	//	default:
	//		assert(0);
	//	}

	double t;
	if (!isZero(y1 - y0)) {
		t = (ymin - y0) / (y1 - y0);
		double z = z0 + t * (z1 - z0);
		if ((isBetween(0.0, 1.0, t)) && (isBetween(zmin, zmax, z)))	return true;

		t = (ymax - y0) / (y1 - y0);
		z = z0 + t * (z1 - z0);
		if ((isBetween(0.0, 1.0, t)) && (isBetween(zmin, zmax, z)))	return true;
	}

	if (!isZero(z1 - z0)) {
		t = (zmin - z0) / (z1 - z0);
		double y = y0 + t*(y1 - y0);
		if ((isBetween(0.0, 1.0, t)) && (isBetween(ymin, ymax, y)))	return true;

		t = (ymax - y0) / (y1 - y0);
		y = y0 + t*(y1 - y0);
		if ((isBetween(0.0, 1.0, t)) && (isBetween(ymin, ymax, y)))	return true;
	}

	return false;
}

static bool edgeIntersectFace(double x0, double y0, double z0, double x1, double y1, double z1,
							  double xmin, double ymin, double ymax, double zmin, double zmax)
{
	double t;
	if (isEqual(x0, xmin) && isEqual(x1, xmin)) { //edge (p0, p1) is in the x = xmin plane
		return edgeIntersectSquare(y0, z0, y1, z1, ymin, ymax, zmin, zmax, 1);
	} else if (!isEqual(x0, x1)) {
		t = (xmin - x0) / (x1 - x0);
		if(isBetween(0, 1, t)) {
			double y = y0 + t*(y1 - y0);
			double z = z0 + t*(z1 - z0);
			if ((isBetween(ymin, ymax, y)) && isBetween(zmin, zmax, z)) {
				return true;
			}
		}
	}	
	return false;
}

static bool edgeIntersectCube(const Vector3& p0, const Vector3& p1, 
							  const Vector3& lower, const Vector3& upper)
{
	double xmin, xmax, ymin, ymax, zmin, zmax;
	xmin = lower[0]; xmax = upper[0];
	ymin = lower[1]; ymax = upper[1];
	zmin = lower[2]; zmax = upper[2];

	//1) face 1 x = xmin...
	if(edgeIntersectFace(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2],xmin,
		ymin, ymax, zmin, zmax)) {
			return true;
	}

	//1) b> face x = xmax...
	if(edgeIntersectFace(p0[0], p0[1], p0[2], p1[0], p1[1], p1[2], xmax,
		ymin, ymax, zmin, zmax)) {
			return true;
	}

	//1) face y = ymin...
	if (edgeIntersectFace(p0[1], p0[0], p0[2], p1[1], p1[0], p1[2], ymin,
		xmin, xmax, zmin, zmax)) {
			return true;
	}

	//1) face y = ymax ...
	if (edgeIntersectFace(p0[1], p0[0], p0[2], p1[1], p1[0], p1[2], ymax,
		xmin, xmax, zmin, zmax)) {
			return true;
	}

	//1) face z = zmin...
	if (edgeIntersectFace(p0[2], p0[0], p0[1], p1[2], p1[0], p1[1], zmin,
		xmin, xmax, ymin, ymax)) {
			return true;
	}

	//1) face z = zmax...
	if (edgeIntersectFace(p0[2], p0[0], p0[1], p1[2], p1[0], p1[1], zmax,
		xmin, xmax, ymin, ymax)) {
			return true;
	}

	return false;
}

bool DistanceTransform::TriangleCubeIntersection(int nt, const Vector3& lower, const Vector3& upper)
{
	Vector3 vert0, vert1, vert2;
	m_pGeoData->getTriangleVerts(nt, vert0, vert1, vert2);
	double x0, y0, z0, x1, y1, z1, x2, y2, z2;

	double xmin, xmax, ymin, ymax, zmin, zmax;
	xmin = lower[0]; xmax = upper[0];
	ymin = lower[1]; ymax = upper[1];
	zmin = lower[2]; zmax = upper[2];

	// find if completely on some side !
	x0 = vert0[0]; 
	y0 = vert0[1]; 
	z0 = vert0[2]; 

	x1 = vert1[0]; 
	y1 = vert1[1];
	z1 = vert1[2];

	x2 = vert2[0]; 
	y2 = vert2[1];
	z2 = vert2[2];

	// If all 3 vertices on same side, then return false;
	if( (x0<xmin) && (x1<xmin) && (x2<xmin) ) return false;
	if( (x0>xmax) && (x1>xmax) && (x2>xmax) ) return false;

	if( (y0<ymin) && (y1<ymin) && (y2<ymin) ) return false;
	if( (y0>ymax) && (y1>ymax) && (y2>ymax) ) return false;

	if( (z0<zmin) && (z1<zmin) && (z2<zmin) ) return false;
	if( (z0>zmax) && (z1>zmax) && (z2>zmax) ) return false;

	//if either of the 3 points are inside the cube, then the Triangle
	// MUST intersect the cube....
	if ( (isBetween(xmin, xmax, x0)) && (isBetween(ymin, ymax, y0)) 
		&& (isBetween(zmin, zmax, z0)) ) return true;
	if ( (isBetween(xmin, xmax, x1)) && (isBetween(ymin, ymax, y1)) 
		&& (isBetween(zmin, zmax, z1)) ) return true;
	if ( (isBetween(xmin, xmax, x2)) && (isBetween(ymin, ymax, y2)) 
		&& (isBetween(zmin, zmax, z2)) ) return true;

	// then the dammed cases when the Triangle intersects any face (edge) 
	// of the cube or vice versa...
	//A) Triangle with the cube...
	// Triangle edge 1
	if (edgeIntersectCube(vert0, vert1, lower, upper)) {
		return true;
	}

	// Triangle edge 2
	if (edgeIntersectCube(vert0, vert2, lower, upper)) {
		return true;
	}

	// Triangle edge 3
	if (edgeIntersectCube(vert1, vert2, lower, upper)) {
		return true;
	}

	///////////////////////////
	//Then the case where the 12 Cube edges intersect the Triangle...
	Vector3 p0, p1;

	// x direction
	p0[0] = xmin;
	p1[0] = xmax;

	p0[1] = p1[1] = ymin;
	p0[2] = p1[2] = zmin;

	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}
	p0[1] = p1[1] = ymax;
	p0[2] = p1[2] = zmin;

	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	p0[1] = p1[1] = ymin;
	p0[2] = p1[2] = zmax;

	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	p0[1] = p1[1] = ymax;
	p0[2] = p1[2] = zmax;

	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	// y direction
	p0[1] = ymin;
	p1[1] = ymax;

	p0[0] = p1[0] = xmin;
	p0[2] = p1[2] = zmin;
	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	p0[0] = p1[0] = xmax;
	p0[2] = p1[2] = zmin;
	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	p0[0] = p1[0] = xmin;
	p0[2] = p1[2] = zmax;
	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	p0[0] = p1[0] = xmax;
	p0[2] = p1[2] = zmax;
	if (rayTriangleIntersection(nt, p0, p1) <= 1) {
		return true;
	}

	// z direction
	p0[2] = zmin;
	p1[2] = zmax;

	p0[0] = p1[0] = xmin;
	p0[1] = p1[1] = ymin;
	if (rayTriangleIntersection(nt, p0, p1)<= 1) {
		return true;
	}

	p0[0] = p1[0] = xmax;
	p0[1] = p1[1] = ymin;
	if (rayTriangleIntersection(nt, p0, p1)<= 1) {
		return true;
	}

	p0[0] = p1[0] = xmin;
	p0[1] = p1[1] = ymax;
	if (rayTriangleIntersection(nt, p0, p1)<= 1) {
		return true;
	}

	p0[0] = p1[0] = xmax;
	p0[1] = p1[1] = ymax;
	if (rayTriangleIntersection(nt, p0, p1)<= 1) {
		return true;
	}

	return false;
}
